#include "vehicleitem.h"
#include "graphicsutilities.h"

#include <QPen>
#include <QDebug>

VehicleItem::VehicleItem(const QString &name, const QPixmap &pixmap)
  : QGraphicsPixmapItem(pixmap)
{
  setZValue(GraphicsUtilities::LAYER_VEHICLE);
  mScale = 0.07f;

  scale(mScale, mScale); //magic numbers?
  translate(-pixmap.width()/2.0, -pixmap.height()/2.0);
  setTransformOriginPoint(pixmap.width()/2.0, pixmap.height()/2.0);

  // Create acceleration line (magenta)
  mAclVectorLine = new QGraphicsLineItem(this);
  mAclVectorLine->setPen(QPen(Qt::magenta, 1.0f/mScale));
  mAclVectorLine->setZValue(GraphicsUtilities::LAYER_VECTORS+2);
  mAclVectorLine->hide();

  // Create velocity line (yellow)
  mVelVectorLine = new QGraphicsLineItem(this);
  mVelVectorLine->setPen(QPen(Qt::yellow, 2.0f/mScale));
  mVelVectorLine->setZValue(GraphicsUtilities::LAYER_VECTORS+1);
  mVelVectorLine->hide();

  // Create engine force (green)
  mEngineForceLine = new QGraphicsLineItem(this);
  mEngineForceLine->setPen(QPen(QColor(0,255,0,200), 2.0f/mScale));
  mEngineForceLine->setZValue(GraphicsUtilities::LAYER_VECTORS);
  mEngineForceLine->hide();

  // Create brake force (red)
  mBrakeForceLine = new QGraphicsLineItem(this);
  mBrakeForceLine->setPen(QPen(QColor(255,0,0,200), 2.0f/mScale));
  mBrakeForceLine->setZValue(GraphicsUtilities::LAYER_VECTORS);
  mBrakeForceLine->hide();

  // Create friction force (black)
  mFrictionForceLine = new QGraphicsLineItem(this);
  mFrictionForceLine->setPen(QPen(QColor(0,0,0,255), 1.0f/mScale));
  mFrictionForceLine->setZValue(GraphicsUtilities::LAYER_VECTORS+3);
  mFrictionForceLine->hide();

  // Create drag force (cyan)
  mDragForceLine = new QGraphicsLineItem(this);
  mDragForceLine->setPen(QPen(QColor(0,255,255,255), 1.0f/mScale));
  mDragForceLine->setZValue(GraphicsUtilities::LAYER_VECTORS+4);
  mDragForceLine->hide();

  // Create drag force (cyan)
  mDragForceLine = new QGraphicsLineItem(this);
  mDragForceLine->setPen(QPen(QColor(0,255,255,255), 1.0f/mScale));
  mDragForceLine->setZValue(GraphicsUtilities::LAYER_VECTORS+4);
  mDragForceLine->hide();

  // Create line to current goal (white)
  mCurrentGoalLine = new QGraphicsLineItem(this);
  mCurrentGoalLine->setPen(QPen(QColor(255,255,255,255), 1.0f/mScale, Qt::DotLine));
  mCurrentGoalLine->setZValue(GraphicsUtilities::LAYER_VECTORS);
  mCurrentGoalLine->hide();

  mBoundingBox = new QGraphicsPolygonItem(QRectF(-7.5,-7.5,15,15),this);
  mBoundingBox->setPen(QPen(QColor(255,255,255,255)));
  //mBoundingBox->setFlag(QGraphicsItem::ItemIgnoresTransformations);
  mBoundingBox->setZValue(GraphicsUtilities::LAYER_VEHICLE);
  mBoundingBox->scale(1.0f/mScale,1.0f/mScale);
  //mBoundingBox->setTransformOriginPoint();
  //mBoundingBox->hide();

  //mLane = new QGraphicsPolygonItem(QRectF(-7.5,-7.5,15,15),this);
  //mLane->setPen(QPen(QColor(255,255,255,255)));
  //mBoundingBox->setFlag(QGraphicsItem::ItemIgnoresTransformations);
  //mLane->setZValue(GraphicsUtilities::LAYER_VEHICLE);
  //mLane->scale(1.0f/mScale,1.0f/mScale);
}

void VehicleItem::setVehicleFrame(VehicleFrame* vf)
{
  if (!vf)
    return;

  setPos(vf->position());
  setRotation(vf->direction());

  if(mBoundingBox)
  {
    mBoundingBox->setRotation(-vf->direction());
    mBoundingBox->setPos(mapFromScene(vf->position()));
  }

  //if(mLane)
  //{
  //  mLane->setRotation(-vf->direction());
  //  mLane->setPos(mapFromScene(vf->position()));
  //}

  if(mAclVectorLine)
    mAclVectorLine->setLine(
        QLineF(mapFromScene(vf->position()), mapFromScene(vf->position()+vf->acceleration())));


  if(mVelVectorLine)
    mVelVectorLine->setLine(
        QLineF(mapFromScene(vf->position()), mapFromScene(vf->position()+vf->velocity())));

  if(mCurrentGoalLine)
  {
    if (vf->currentGoal() != Point(0, 0))
      mCurrentGoalLine->setLine(
          QLineF(mapFromScene(vf->position()), mapFromScene(vf->currentGoal())));
  }

  Point eng, brk, fric, drag;
  vf->forces(eng, brk, fric, drag);
  eng = eng * 0.01;
  brk = brk * 0.01;
  fric = fric * 0.01;
  drag = drag * 0.01;

  if(mEngineForceLine)
  {
    //mEngineForceLine->setVisible(eng.magnitude()>0.0f);
    mEngineForceLine->setLine(
        QLineF(mapFromScene(vf->position()), mapFromScene((vf->position()+eng))));
  }

  if(mBrakeForceLine)
  {
    //mBrakeForceLine->setVisible(mShowForces && brk.magnitude()>1.0f);
    mBrakeForceLine->setLine(
        QLineF(mapFromScene(vf->position()), mapFromScene(vf->position()+brk)));
  }
  if(mFrictionForceLine)
  {
    //mFrictionForceLine->setVisible(mShowForces && fric.magnitude()>1.0f);
    mFrictionForceLine->setLine(
        QLineF(mapFromScene(vf->position()), mapFromScene(vf->position()+fric)));
  }
  if(mDragForceLine)
  {
    //mDragForceLine->setVisible(drag.magnitude()>1.0f);
    mDragForceLine->setLine(
        QLineF(mapFromScene(vf->position()), mapFromScene(vf->position()+ drag)));
  }
}

void VehicleItem::showForces(const bool show)
{
  if(mEngineForceLine)
    mEngineForceLine->setVisible(show);
  if(mBrakeForceLine)
    mBrakeForceLine->setVisible(show);
  if(mFrictionForceLine)
    mFrictionForceLine->setVisible(show);
  if(mDragForceLine)
    mDragForceLine->setVisible(show);
}

void VehicleItem::showVectors(const bool show)
{
  if(mVelVectorLine)
    mVelVectorLine->setVisible(show);
  if(mAclVectorLine)
    mAclVectorLine->setVisible(show);
  if(mCurrentGoalLine)
    mCurrentGoalLine->setVisible(show);
  if(mBoundingBox)
    mBoundingBox->setVisible(show);
}
